#include "Precompile.h"
#include "AnimationController.h"

namespace Graphics{
	AnimationController::AnimationController(std::vector<SkeletonJointPtr> &joints, Utility::Math::Matrix<4,4>& bindMatrix, std::string& name){
		m_name = name;
		m_joints = joints;
		m_bindMatrix = bindMatrix;

		Utility::Math::Matrix<4,4> scaleMatrix,translateMatrix,rotationMatrix,animRotationMatrix,resultMatrix;
		for(unsigned int i=0; i<m_joints.size(); i++){
			m_jointNameMap[m_joints[i]->m_name] = m_joints[i];
			if(!m_joints[i]->m_bakedTransform){
				Utility::Math::Quaternion<float> orientX,orientY,orientZ,rotX,rotY,rotZ;

				orientX.createFromAxisAngle(Utility::Math::Vector<>(m_joints[i]->m_orientAngleX.a),m_joints[i]->m_orientAngleX(Utility::Math::_w_)*(E_PI/180.0f));
				rotX.createFromAxisAngle(Utility::Math::Vector<>(m_joints[i]->m_rotateAngleX.a),m_joints[i]->m_rotateAngleX(Utility::Math::_w_)*(E_PI/180.0f));

				orientY.createFromAxisAngle(Utility::Math::Vector<>(m_joints[i]->m_orientAngleY.a),m_joints[i]->m_orientAngleY(Utility::Math::_w_)*(E_PI/180.0f));
				rotY.createFromAxisAngle(Utility::Math::Vector<>(m_joints[i]->m_rotateAngleY.a),m_joints[i]->m_rotateAngleY(Utility::Math::_w_)*(E_PI/180.0f));

				orientZ.createFromAxisAngle(Utility::Math::Vector<>(m_joints[i]->m_orientAngleZ.a),m_joints[i]->m_orientAngleZ(Utility::Math::_w_)*(E_PI/180.0f));
				rotZ.createFromAxisAngle(Utility::Math::Vector<>(m_joints[i]->m_rotateAngleZ.a),m_joints[i]->m_rotateAngleZ(Utility::Math::_w_)*(E_PI/180.0f));

				rotationMatrix.createFromQuaternion(orientZ*orientY*orientX);
				rotationMatrix.transpose();

				animRotationMatrix.createFromQuaternion(rotZ*rotY*rotX);
				animRotationMatrix.transpose();
			
				translateMatrix.identity();
				translateMatrix.set(3,0,m_joints[i]->m_translate(Utility::Math::_x_));
				translateMatrix.set(3,1,m_joints[i]->m_translate(Utility::Math::_y_));
				translateMatrix.set(3,2,m_joints[i]->m_translate(Utility::Math::_z_));
				scaleMatrix.identity();
				scaleMatrix.set(0,0,m_joints[i]->m_scale(Utility::Math::_x_));
				scaleMatrix.set(1,1,m_joints[i]->m_scale(Utility::Math::_y_));
				scaleMatrix.set(2,2,m_joints[i]->m_scale(Utility::Math::_z_));

				resultMatrix = rotationMatrix*animRotationMatrix*scaleMatrix*translateMatrix;
			}
			else
				resultMatrix = m_joints[i]->m_transformMatrix;

			m_bindPoseMatrices.push_back(resultMatrix);
		}

		for(unsigned int i=0; i<m_bindPoseMatrices.size(); i++){
			for(unsigned int j=0; j<m_joints[i]->m_children.size(); j++){
				m_bindPoseMatrices[m_joints[i]->m_children[j]] *= m_bindPoseMatrices[i];
			}
			m_bindPoseMatrices[i] = m_bindMatrix * m_joints[i]->m_invBindMatrix * m_bindPoseMatrices[i];
		}
	}

	void AnimationController::computeJointFrame(std::string& clipName, float time, std::vector<Utility::Math::Matrix<4,4>>& joints, RenderContext* context){
		std::map<std::string,unsigned int>::iterator itr = m_animationClips.find(clipName);
		if(itr == m_animationClips.end()){
			LOG_ERROR("Clip with that name does not exist in this controller.");
			assert(0);
			return;
		}

		computeJointFrame(itr->second,time,joints,context);
	}
	
	void AnimationController::computeJointFrame(int clipId, float time, std::vector<Utility::Math::Matrix<4,4>>& joints, RenderContext* context){
		Graphics::AnimationClipPtr clip = context->m_animationManager->getAnimationClip(clipId);
		if(!clip){
			LOG_ERROR("Clip with that id does not exist.");
			assert(0);
			return;
		}

		clip->computeClipJointFrame(time,joints,m_joints,context);

		// Premultiply joint transform matrix and bind inverse matrix
		for(unsigned int i=0; i<joints.size(); i++){
			joints[i] = m_bindMatrix * m_joints[i]->m_invBindMatrix * joints[i];
		}
	}

	void AnimationController::computeJointFrame(int sourceClip,int targetClip,float sourceTime, float targetTime, float interpolate, std::vector<Utility::Math::Matrix<4,4>>& joints, RenderContext* context){
		Graphics::AnimationClipPtr sClip = context->m_animationManager->getAnimationClip(sourceClip);
		Graphics::AnimationClipPtr tClip = context->m_animationManager->getAnimationClip(targetClip);
		if(!sClip || !tClip){
			LOG_ERROR("Clip with that id does not exist.");
			assert(0);
			return;
		}

		sClip->computeClipJointFrame(sourceTime,targetTime,interpolate,tClip,joints,m_joints,context);

		// Premultiply joint transform matrix and bind inverse matrix
		for(unsigned int i=0; i<joints.size(); i++){
			joints[i] = m_bindMatrix * m_joints[i]->m_invBindMatrix * joints[i];
		}
	}

	void AnimationController::addAnimationClip(std::string &name, unsigned int id){
		m_animationClips[name] = id;
	}

	SkeletonJointPtr AnimationController::getJoint(std::string &name){
		std::map<std::string,SkeletonJointPtr>::iterator itr;
		if((itr = m_jointNameMap.find(name)) == m_jointNameMap.end()){
			LOG_ERROR("Joint with that name not found.");
			assert(0);
			return SkeletonJointPtr();
		}

		return itr->second;
	}

	std::vector<std::string> AnimationController::getClips(){
		std::vector<std::string> vec;

		std::map<std::string,unsigned int>::iterator itr = m_animationClips.begin();
		while(itr != m_animationClips.end()){
			vec.push_back(itr->first);
			itr++;
		}

		return vec;
	}

	std::vector<std::string> AnimationController::getJointNames(){
		std::vector<std::string> vec;

		std::map<std::string,SkeletonJointPtr>::iterator itr = m_jointNameMap.begin();
		while(itr != m_jointNameMap.end()){
			vec.push_back(itr->first);			
			itr++;
		}

		return vec;
	}

	int AnimationController::getClipId(std::string& name){
		std::map<std::string,unsigned int>::iterator itr = m_animationClips.find(name);
		if(itr == m_animationClips.end()){
			LOG_ERROR("Clip with that name does not exist in this controller.");
			assert(0);
			return -1;
		}

		return itr->second;
	}
}